#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "cg.h"

void cg_point4_from_point3(cgPoint4 *p4, cgPoint *p3)
{
    memcpy(p4->c, p3->c, 3*sizeof(double));
    p4->c[3] = 1.0;
}

void cg_point3_from_point4(cgPoint *p3, cgPoint4 *p4)
{
    int i;
    for (i = 0; i < 3; i++)
	p3->c[i] = p4->c[i] / p4->c[3];
}

TransformMatrix *cg_identity_matrix(TransformMatrix *tm)
{
    TransformMatrix *buf = tm;
    int i;
    if (buf == NULL)
    	buf = (TransformMatrix *)malloc(sizeof(TransformMatrix));
    memset(buf, 0, sizeof(TransformMatrix));
    for (i = 0; i < 4; i++)
	(*buf)[i][i] = 1.0;
    return buf;
}

TransformMatrix *cg_rot_matrix_x(double angle, TransformMatrix *tm)
{
    TransformMatrix *buf = cg_identity_matrix(tm);
    (*buf)[1][1] = cos(angle);
    (*buf)[1][2] = -sin(angle);
    (*buf)[2][1] = sin(angle);
    (*buf)[2][2] = cos(angle);
    return buf;
}

TransformMatrix *cg_rot_matrix_y(double angle, TransformMatrix *tm)
{
    TransformMatrix *buf = cg_identity_matrix(tm);
    (*buf)[0][0] = cos(angle);
    (*buf)[0][2] = sin(angle);
    (*buf)[2][0] = -sin(angle);
    (*buf)[2][2] = cos(angle);
    return buf;
}

TransformMatrix *cg_rot_matrix_z(double angle, TransformMatrix *tm)
{
    TransformMatrix *buf = cg_identity_matrix(tm);
    (*buf)[0][0] = cos(angle);
    (*buf)[0][1] = sin(angle);
    (*buf)[1][0] = -sin(angle);
    (*buf)[1][1] = cos(angle);
    return buf;
}

TransformMatrix *cg_translate_matrix(
    double x, 
    double y, 
    double z, 
    TransformMatrix *tm)
{
    TransformMatrix *buf = cg_identity_matrix(tm);
    (*buf)[0][3] = x;
    (*buf)[1][3] = y;
    (*buf)[2][3] = z;
    return buf;
}

TransformMatrix *cg_zoom_matrix(
    double x, 
    double y, 
    double z, 
    TransformMatrix *tm)
{
    TransformMatrix *buf = cg_identity_matrix(tm);
    (*buf)[0][0] = x;
    (*buf)[1][1] = y;
    (*buf)[2][2] = z;
    return buf;
}

void cg_mult_matrix(TransformMatrix *tm1, TransformMatrix *tm2)
{
    int i, j, k;
    TransformMatrix buf;
    memcpy(&buf, tm1, sizeof(TransformMatrix));
    memset(tm1, 0, sizeof(TransformMatrix));
    for (i = 0; i < 4; i++)
	for (j = 0; j < 4; j++)
	    for (k = 0; k < 4; k++)
		(*tm1)[i][j] += buf[i][k] * (*tm2)[k][j];
}

void cg_rotate(TransformMatrix *tm, int axis, double angle)
{
    TransformMatrix rm;
    switch (axis)
    {
    case X:
	cg_rot_matrix_x(angle, &rm);
	break;
    case Y:
	cg_rot_matrix_y(angle, &rm);
	break;
    case Z:
	cg_rot_matrix_z(angle, &rm);
	break;
    }
    cg_mult_matrix(tm, &rm);
}

void cg_translate(TransformMatrix *tm, double x, double y, double z)
{
    TransformMatrix trm;
    cg_translate_matrix(x, y, z, &trm);
    cg_mult_matrix(tm, &trm);
}

void cg_zoom(TransformMatrix *tm, double x, double y, double z)
{
    TransformMatrix zm;
    cg_zoom_matrix(x, y, z, &zm);
    cg_mult_matrix(tm, &zm);
}

void cg_transform_point(cgPoint *dest, cgPoint *src, TransformMatrix *tm)
{
    int i, j;
    cgPoint4 src4, dest4;
    cg_point4_from_point3(&src4, src);
    memset(dest4.c, 0, 4*sizeof(double));

    for (i = 0; i < 4; i++)
	for (j = 0; j < 4; j++)
	    dest4.c[i] += (*tm)[i][j] * src4.c[j];
    cg_point3_from_point4(dest, &dest4);
}

void cg_matrix_to_opengl(double *otm, TransformMatrix *tm)
{
    int i, j;
    for (i = 0; i < 4; i++)
	for (j = 0; j < 4; j++)
	    otm[i*4 + j] = (*tm)[j][i];
}

void cg_transform_points(cgPoint *buf, int size, TransformMatrix *tm)
{
    int i;
    for (i = 0; i < size; i++)
	cg_transform_point(buf+i, buf+i, tm);
}

void cg_transform_lines(cgLine *buf, int size, TransformMatrix *tm)
{
    int i;
    for (i = 0; i < size; i++)
	cg_transform_points(buf[i].p, 2, tm);
}

void cg_transform_triangles(cgTriangle *buf, int size, TransformMatrix *tm)
{
    int i;
    for (i = 0; i < size; i++)
	cg_transform_points(buf[i].p, 3, tm);
}

void cg_transform_grid(cgPoint **buf, int w, int h, TransformMatrix *tm)
{
    int i;
    for (i = 0; i < h; i++)
	cg_transform_points(buf[i], w, tm);
}
